%author: wxj233
%time: 2023.11.9 10:00
%function:  追踪器

classdef MFPTracer < handle
    %FEATURETRACER 多特征点追踪器
    %   此处显示详细说明
    
    properties
        Ips  % initPoolPoints,起始池用于存放用于起始轨迹的点，[1t, 2x, 3y, 4id(仿真真实目标id); ...]

        Tracks  % initTracks,初始轨迹,[track1, track2, ...]

        lastTime  % 上一个时刻
        nowTime  % 当前时刻
        times % 用于保存时序的队列，主要用于3/4逻辑法的判断

        vMax  % 目标最大速度
        dr  % 距离量测方差
        dtheta  % 角度量测方差
        dvr  % 径向速度量测方差
        q  % 过程噪声量级
        p  % Velocity proportion  自身速度在融合速度中所占的比例
        vAssDoor  % 特征点速度关联概率门限
        inDoor  % 数据关联门限（落入波门内的概率）
        fplifeMax  % feature point life,特征点最大生命值
        tlifeMax  % track life,轨迹当前生命值

        epsilon  % dbscan 聚类半径
        minpts  % dbscan聚类核心数
        zoomX  % x轴缩放参数
        zoomY  % y轴缩放参数
        zoomV  % vr缩放参数
        extR  % 扩展半径
        amend  % 修正系数

        deadFps  % 用于记录死亡的特征点轨迹
    end
    
    methods
        function obj = MFPTracer(vMax, dr, dvr, dtheta, q, p, vAssDoor, inDoor, fplifeMax, tlifeMax, extR, amend)
            % 构造此类的实例
            % 完成初始化工作
            % vMax 最大速度
            % r: 距离量测方差
            % dtheta: 角度量测方差
            % dvr: 径向速度量测方差
            % q: 过程噪声数量级
            % vp: 自身速度在融合速度中所占的比例取[0, 1]之间,0是不相关，1是强相关
            % vAssDoor 速度关联波门
            % inDoor: 数据关联门限（落入波门内的概率）
            % fplifeMax 特征点最大生命值
            % tlifeMax 轨迹最大生命值
            % extR 扩展半径
            % amend  修正系数
            obj.Ips = [];
            obj.Tracks = struct([]);
            obj.deadFps = struct([]);
            obj.lastTime = 0;
            obj.nowTime = 0;
            obj.times = [];

            obj.vMax = vMax;
            obj.dr = dr;
            obj.dtheta = dtheta;
            obj.dvr = dvr;
            obj.q = q;
            obj.p = p;
            obj.vAssDoor = vAssDoor;
            obj.inDoor = inDoor;
            obj.fplifeMax = fplifeMax;
            obj.tlifeMax = tlifeMax;
            obj.extR = extR;
            obj.amend = amend;

            Track.getId(0);  % 初始化轨迹ID编号为0
            FeaturePoint.geId(0);  % 初始化特征点ID编号为0
        end


        function points = preproccess(obj, points, epsilon, minpts, zoomX, zoomY, zoomV)
            % 进行数据预处理，聚类，噪声滤除等
            % points 待处理量测[t, r, theta, vr, x, y, vx, vy, id]
            % epsilon 聚类半径
            % minpts 聚类核最小量测数
            % zoomX: x轴缩放
            % zoomY: y轴缩放
            % zoomV: 径向速度缩放
            % return points: 预处理之后的点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            if isempty(points)
                return;
            end

            X = points(:, 2).*cos(points(:, 3));
            Y = points(:, 2).*sin(points(:, 3));

            obj.epsilon = epsilon;
            obj.minpts = minpts;
            obj.zoomX = zoomX;
            obj.zoomY = zoomY;
            obj.zoomV = zoomV;
            cluster = dbscan([zoomX*X, zoomY*Y, zoomV*points(:, 4)], epsilon, minpts,"Distance","euclidean");
            points = [points, X, Y, cluster];
        end


        function tracer(obj, points)
            % 追踪入口，在此处完成轨迹关联、
            % 此处显示详细说明
            % points 待处理的点 [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            if isempty(points)
                return;
            end
            obj.lastTime = obj.nowTime;
            obj.nowTime = points(1,1);

            offPs = obj.trackAss(points);  % 初始轨迹关联
            offPs = obj.trackExtend(offPs);  % 轨迹拓展
            offPs = obj.trackInit(offPs);  % 轨迹起始
            
            if length(obj.times) >= 2
                obj.Ips(obj.Ips(:,1)==obj.times(1), :) = [];  % 删除上上个时刻的点
                obj.times(1) = [];  % 删除上上个时刻
            end
            
            obj.Ips = [obj.Ips; offPs];  % 最终剩余的游离点放入起始池
            obj.times = [obj.times, obj.nowTime];
        end


        function offPs = trackAss(obj, points)
            % 完成轨迹关联
            % points: 待关联的点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            % return offPs: 返回没有关联成功的离群点
            if isempty(obj.Tracks)
                offPs = points;
                return;
            end
            dt = obj.nowTime - obj.lastTime;
            fps = struct([]);  % 用于保存所有track中的fps的引用
            costM = [];  % 代价矩阵，用于保存特征点到每一个量测之间的代价(距离)
            for t = 1:1:length(obj.Tracks)
                obj.Tracks(t).vfusion(obj.p);
                obj.Tracks(t).predict(dt, obj.q);  % 先完成预测才可以求得信息这些东西
                for i = 1:1:length(obj.Tracks(t).fps)
                    fp = obj.Tracks(t).fps(i);  % 获取到单个特征点
                    costs = fp.costs(points, obj.inDoor, obj.dr, obj.dtheta, obj.dvr);  % 代价计算
                    costM = [costM; costs];
                    fps = [fps, fp];  % 建立对当前特征点的索引，方便后续读取
                end
                obj.Tracks(t).validFpNum = 0;  % 新一轮的关联开始了，原状态置0
            end

            [ass, costM] = obj.hungarian(-costM);  % 匹配最优,转换为最小代价
            % 匹配修正,大家都匹配的很好，你一个人太差就不行
            [ass, costM] = obj.matchAmend(obj.Tracks, fps, ass, costM);
            useP = [];
            for i = 1:1:length(fps)
                if costM(i, ass(i)) < 0  % 有可关联点
                    fps(i).assP = points(ass(i), :);
                    useP = [useP, ass(i)];
                    fps(i).track.validFpNum = fps(i).track.validFpNum + 1;
                else
                    fps(i).assP = [];
                end
            end

            % 更新
            for track = obj.Tracks
                track.update(obj.dr, obj.dtheta, obj.dvr, obj.nowTime);
            end

            points(useP, :) = [];  % 去除已关联的点
            offPs = points;
        end

        function [ass, costM] = matchAmend(obj, tracks, fps, ass, costM)
            % 匹配修正
            % tracks: 所有轨迹
            % fps: 所有散射点轨迹
            % ass: 每一个fp对应的
            for track = tracks
                sum_c = 0;
                validNum = 0;
                for i =1:1:length(fps)
                    if fps(i).track.id == track.id && costM(i, ass(i)) < 0  % 有可关联点
                        sum_c = sum_c + costM(i, ass(i));
                        validNum = validNum+1;
                    end
                end

                for i =1:1:length(fps)
                    if fps(i).track.id == track.id && costM(i, ass(i)) < 0  % 有可关联点
                        if (sum_c/validNum)/costM(i, ass(i))*obj.p > obj.amend % 如果不到均值代价的1/3则说明误差比较大了，就放弃。
                            costM(i, ass(i)) = 0;
                        end
                    end
                end

            end
        end

        function offPs = trackExtend(obj, points)
            % 轨迹特征点拓展
            % points: 用于关联的点,[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            if isempty(obj.Tracks) || isempty(points) || isempty(obj.Ips)
                offPs = points;
                return;
            end

            INum = size(obj.Ips, 1);  % 初始池点数量
            mNum = size(points, 1);  % 量测点数量
            costM = zeros([INum, mNum]);  % 代价矩阵
            VM = cell([INum, mNum]);  % 速度元组矩阵
            PM = cell([INum, mNum]);  % 速度协方差元组矩阵
            for i = 1:1:INum
                for j = 1:1:mNum
                    [cost, V, Pv] = obj.isInDoor(obj.Ips(i, :), points(j, :));
                    costM(i, j) = cost;  % 取全局最小代价即可，就不会出现重复点使用了
                    VM{i, j} = V;
                    PM{i, j} = Pv;
                end
            end
            [ass, costM] = obj.hungarian_V(costM);
            dI = [];  % 记录需要被删除的最新量测点
            diI = [];  % 记录需要被删除的起始池中的点

            for i = 1:1:INum
                if costM(i, ass(i)) < (1e2-1)  % 说明成功关联点,-1是为了防止精度问题
                    diff_values = [];
                    Ds = [];
                    TI = [];
                    for j = 1:1:length(obj.Tracks)
                        track=obj.Tracks(j);
                        gps = reshape(track.gXk_e, 4, [])'; % 群状态中的所有点[x, vx, y, vy;...]
                        gps = [gps, sum(gps(:, [2,4]).*gps(:, [1,3]), 2)./sqrt(sum(gps(:, [1,3]).*gps(:, [1,3]), 2))];  % [x, vx, y, vy, vr;...]\

                        diff = (points(ass(i), [10, 11, 4]) - gps(:,[1, 3, 5])).*[obj.zoomX, obj.zoomY, obj.zoomV];
                        diff_value = min(vecnorm(diff,2,2));

%                         mp = track.points(end, 2:5);
%                         estimate = [mp([1,3]), mp([1,3])*mp([2,4])'/norm(mp([1,3]))];  % 质心的[x, y, vr;...]
%                         diff = (estimate-points(ass(i), [10, 11, 4])).*[obj.zoomX, obj.zoomY, obj.zoomV];
%                         diff_value = norm(diff, 2);

                        if diff_value < obj.extR  % 在距离门限内
                            V1 = VM{i, ass(i)};
                            P1 = PM{i, ass(i)};
                            vdiff = track.points(end, [3,5])' - V1;
                            var = track.mPcov([2,4], [2,4]) + P1;
                            D = vdiff'/var*vdiff;
                            if D < chi2inv(0.95, 2)
                                diff_values = [diff_values, diff_value];
                                Ds = [Ds, D];
                                TI = [TI, j];  % 记录是哪一个轨迹
                            end
                        end
                    end
                    if ~isempty(TI)
                        dI = [dI, ass(i)];
                        diI = [diI, i];
                        diff_values = diff_values/sum(diff_values);  % 归一化
                        Ds = Ds/sum(Ds); % 归一化
                        costs = diff_values+Ds;
                        [~, I]=min(costs);
                        % 进行拓展
                        obj.Tracks(TI(I)).addFp(obj.Ips(i, :), points(ass(i), :), obj.dr, obj.dtheta);
                    end
                end
            end


            obj.Ips(diI, :) = [];  % 起始池中被用于扩展的点，删除
%             dI = unique(dI);
            points(dI, :) = [];
            offPs = points;
        end


        function offPs = trackInit(obj, points)
            % 轨迹初始化
            % points: 用于轨迹初始化的点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            if ~isempty(obj.Ips)
                INum = size(obj.Ips, 1);  % 初始池点数量
                mNum = size(points, 1);  % 量测点数量
                costM = zeros([INum, mNum]);  % 代价矩阵
                VM = cell([INum, mNum]);  % 速度元组矩阵
                PM = cell([INum, mNum]);  % 速度协方差元组矩阵
                for i = 1:1:INum
                    for j = 1:1:mNum
                        [cost, V, Pv] = obj.isInDoor(obj.Ips(i, :), points(j, :));
                        costM(i, j) = cost;  % 取全局最小代价即可，就不会出现重复点使用了
                        VM{i, j} = V;
                        PM{i, j} = Pv;
                    end
                end
                [ass, costM] = obj.hungarian_V(costM);
                assedI = [];  % associated velocity记录已经完成速度关联的初始点的索引
                assedP = [];  % points中用于记录已经被关联了的点
                for i = 1:1:INum
                    if ismember(i, assedI)
                        continue;
                    end
                    track = Track(obj.tlifeMax, obj);  % 创建一个新的轨迹
                    if costM(i, ass(i)) < (1e2-1)  % 说明成功关联点,-1是为了防止精度问题
                        V1 = VM{i, ass(i)};
                        P1 = PM{i, ass(i)};
                        for j = 1:1:INum
                            if ismember(j, assedI)
                                continue;
                            end
                            if costM(j, ass(j)) < (1e2-1)  % 说明成功关联点,-1是为了防止精度问题
                                V2 = VM{j, ass(j)};
                                P2 = PM{j, ass(j)};
                                v_diff = V1 - V2;
                                P_diff = P1 + P2;
                                D = v_diff'/P_diff*v_diff;
                                isSameCluster = points(ass(i), 12) == points(ass(j), 12);
                                if isSameCluster && D < chi2inv(obj.vAssDoor, 2)  % 这个地方可以思考下，如果刚好是哪个5%没匹配上怎么办，会出现多个轨迹
                                    track.addFp(obj.Ips(j, :), points(ass(j), :), obj.dr, obj.dtheta);
                                    assedI = [assedI, j];
                                    assedP = [assedP, ass(j)];
                                end
                            end
                        end
                        obj.Tracks = [obj.Tracks, track];
                    end
                end
                points(assedP, :) = []; % 删除被用了的点
                obj.Ips(assedI, :) = []; % 删除起始池中被关联了的点
            end
            offPs = points;  % 返回起始后剩余的游离点
        end


        function [cost, V, Pv] = isInDoor(obj, P1, P2)
            % 初始波门判断,判断P2是否在P1的波门内
            % P1: 起始点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster]
            % P2: 需要判断是否可以关联的点（是否在P1波门内）[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            dt = P2(1) - P1(1);
            Vr1 = P1(4);
            Vr2 = P2(4);

            Xe = [P1([2, 3]), P2([2, 3])]';
            Pe = [obj.dr, 0, 0, 0;
                0, obj.dtheta, 0, 0;
                0, 0, obj.dr, 0;
                0, 0, 0, obj.dtheta];
            [V, Pv, ~] = UT(@obj.calV, Xe, Pe, 2, dt);

            if abs(Vr1)>obj.vMax || abs(Vr2)>obj.vMax || norm(V)>obj.vMax
                cost = 1e2;
                return;
            end

            
            [Vr1_e, Pv1, ~] = UT(@obj.calVr1, Xe, Pe, 1, dt);
            [Vr2_e, Pv2, ~] = UT(@obj.calVr2, Xe, Pe, 1, dt);

            D1 = (Vr1 - Vr1_e)^2/(Pv1+obj.dvr);
            D2 = (Vr2 - Vr2_e)^2/(Pv2+obj.dvr);
            threshold = chi2inv(0.95, 1);
            if D1 < threshold && D2 < threshold
                cost=max([D1, D2]);  % 这一步取最大值简直牛逼，后面就可以直接使用全局最优完成匹配了
            else
                cost = 1e2;
            end
        end


        function V = calV(~, Xe, dt)
            % Xe: [r1, theta1, r2, theta2]'
            % dt: 时间差
            % return V: 速度
            L1 = [Xe(1,1)*cos(Xe(2,1)), Xe(1,1)*sin(Xe(2,1))];
            L2 = [Xe(3,1)*cos(Xe(4,1)), Xe(3,1)*sin(Xe(4,1))];
            V = (L2 - L1)/dt;
            V = V';
        end


        function Vr = calVr1(~, Xe, dt)
            % Xe: [r1, theta1, r2, theta2]'
            % dt: 时间差
            L1 = [Xe(1,1)*cos(Xe(2,1)), Xe(1,1)*sin(Xe(2,1))];
            L2 = [Xe(3,1)*cos(Xe(4,1)), Xe(3,1)*sin(Xe(4,1))];
            V = (L2 - L1)/dt;
            Vr = V*L1'/norm(L1);
        end


        function Vr = calVr2(~, Xe, dt)
            % Xe: [r1, theta1, r2, theta2]'
            % dt: 时间差
            L1 = [Xe(1,1)*cos(Xe(2,1)), Xe(1,1)*sin(Xe(2,1))];
            L2 = [Xe(3,1)*cos(Xe(4,1)), Xe(3,1)*sin(Xe(4,1))];
            V = (L2 - L1)/dt;
            Vr = V*L2'/norm(L2);
        end
        

        function [ass, costM] = hungarian(~, costM)
            % 匈牙利算法实现最佳匹配
            % costM: 代价矩阵
            % return 匹配情况[t, j; ...]第t个特征点轨迹匹配第j个点
            [fpt, pNum] = size(costM);  % 获取特征点轨迹数量和待关联点数量
            if fpt > pNum  % 补成方阵
                num = fpt - pNum;
                patch = zeros([fpt, num]);  % 0代表不可能
                costM = [costM, patch];
            else
                num = pNum - fpt;
                patch = zeros([num, pNum]);  % 0代替不可能
                costM = [costM; patch];
            end
            [ass, ~] = munkres(costM);
        end


        function [ass, costM] = hungarian_V(~, costM)
            % 匈牙利算法实现最佳匹配
            % costM: 代价矩阵
            % return 匹配情况[t, j; ...]第t个特征点轨迹匹配第j个点
            [fpt, pNum] = size(costM);  % 获取特征点轨迹数量和待关联点数量
            if fpt > pNum  % 补成方阵
                num = fpt - pNum;
                patch = 1e2*ones([fpt, num]);  % 100代表不可能
                costM = [costM, patch];
            else
                num = pNum - fpt;
                patch = 1e2*ones([num, pNum]);  % 100代替不可能
                costM = [costM; patch];
            end
            [ass, ~] = munkres(costM);
        end


        function deleteTrack(obj, id)
            % 删除某一条指定轨迹
            % id: 轨迹id
            for t = 1:1:length(obj.Tracks)
                if obj.Tracks(t).id == id
                    for fp = obj.Tracks(t).fps
                        obj.deadFps = [obj.deadFps, fp];
                    end
                    obj.Tracks(t) = [];
                    return;
                end
            end
        end

    end
end

